#include "SerialControl.h"
#include <iostream>

SerialControl::SerialControl(const std::string& port, int baudrate, int timeout) {
    try {
        serial_.setPort(port);
        serial_.setBaudrate(baudrate);
        serial::Timeout t = serial::Timeout::simpleTimeout(timeout);
        serial_.setTimeout(t);
        serial_.open();

        // 初始化ID映射表
        for (int i = 0; i < 30; ++i) {
            std::ostringstream oss;
            oss << std::setw(3) << std::setfill('0') << i;
            idMap_[std::to_string(i)] = oss.str();
        }

        if (serial_.isOpen()) {
            std::cout << "Serial port opened, self-checking..." << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1500));
            std::cout << "Self-check completed. Ready for commands." << std::endl;
            std::cout << "port: " << serial_.getPort() << std::endl;
            std::cout << "baudrate: " << serial_.getBaudrate() << std::endl;
        }
    } catch (const std::exception& e) {
        std::cerr << "Serial init error: " << e.what() << std::endl;
    }
}

SerialControl::~SerialControl() {
    closeSerial();
}

std::string SerialControl::generateWriteCommand(const std::vector<double>& rads, const std::string& machine) {
    if (machine == "arm") {
        std::vector<int> pwmList;
        for (size_t i = 0; i < rads.size(); ++i) {
            if (i != 3) {
                pwmList.push_back(1500 - static_cast<int>(std::round(rads[i] * 4000 / (3 * M_PI))));
            } else {
                pwmList.push_back(static_cast<int>(std::round(rads[i] * 4000 / (3 * M_PI)) + 1500));
            }
        }

        std::ostringstream cmd;
        cmd << "{";
        for (size_t i = 0; i < pwmList.size(); ++i) {
            cmd << "#" << idMap_[std::to_string(i + 21)] 
                << "P" << pwmList[i] << "T1500!";
        }
        cmd << "}";
        return cmd.str();
    }
    return "";
}

std::vector<std::string> SerialControl::generateReadCommand(const std::string& machine) {
    std::vector<std::string> commands;
    if (machine == "arm") {
        for (int i = 0; i < 5; ++i) {
            std::ostringstream cmd;
            cmd << "#" << idMap_[std::to_string(i + 21)] << "PRAD!";
            commands.push_back(cmd.str());
        }
    }
    return commands;
}

void SerialControl::sendCommand(const std::vector<double>& rads, const std::string& machine) {
    if (!serial_.isOpen()) return;

    std::string command = generateWriteCommand(rads, machine);
    // std::cout << command << std::endl;
    if (!command.empty()) {
        serial_.write(command);
        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    }
}

void SerialControl::sendCommand(const std::string command)
{
    serial_.write(command);
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
}

std::vector<double> SerialControl::getRadsData(const std::string& machine) {
    std::vector<std::string> feedbacks;
    std::vector<std::string> commands = generateReadCommand(machine);
    
    for (std::string cmd : commands) {
        try
        {
            serial_.write(cmd);
            std::cout << "Sent data: " << cmd << std::endl;

            uint8_t buffer[256];  // 修改为 uint8_t 类型
            size_t bytesRead = serial_.read(buffer, sizeof(buffer));
            if (bytesRead > 0) 
            {
                buffer[bytesRead] = '\0';  // 确保字符串以 null 结尾
                std::string response = std::string(reinterpret_cast<char*>(buffer));
                std::cout << "Received data: " << response << std::endl;
                feedbacks.push_back(response);  // 保存响应
            } 
            else 
            {
                std::cout << "No data received." << std::endl;
            }

        }
        catch (const serial::IOException& e) 
        {
            std::cerr << "Error while sending data: " << e.what() << std::endl;
        }
    }

    return parsePWMFeedback(feedbacks);
}


std::vector<double> SerialControl::parsePWMFeedback(const std::vector<std::string>& feedbacks) {
    std::vector<double> radList;
    for (const auto& fb : feedbacks) {
        if (fb.length() < 5) continue;
        
        std::string pwmStr = fb.substr(fb.size() - 5, 4);
        try {
            int pwm = std::stoi(pwmStr);
            if (pwm >= 500 && pwm <= 2500) {
                double rad;
                if (radList.size() != 3) { // 索引判断
                    rad = (1500 - pwm) * 3 * M_PI / 4000;
                } else {
                    rad = (pwm - 1500) * 3 * M_PI / 4000;
                }
                radList.push_back(rad);
            }
        } catch (...) {
            // 忽略转换错误
        }
    }
    return radList;
}

void SerialControl::closeSerial() {
    if (serial_.isOpen()) {
        serial_.close();
        std::cout << "Serial port closed." << std::endl;
    }
}
